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ABSTRACT 


The error inhxxiuced by the DF accuracy of ttiree Direction Hnding equi{»nents used 
on board two surfoce units providing bearing rx^surements is used to evaluate the 
performance of the ESM systems through the Extended Kalman Rl^ equations, and to 
appreciate the result in die estimation process by evaluating how the unprov«ne»t of the 
tracking process is reduced as the error introduced by the DF accuracy of the 
systems is increased. 

The process is conducted at three different sceiuuios involving a dtange in coune 
of 0, 45 and 90 degrees, and it can be seen how the maneuver detection algorithm stop 
working when the DF accuracy reachs 4.6 deg. for foe second scenario and 9.6 deg. for the 
scenario Nr 3. 
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L INTRODUCTION 


A. DESCRIPTION OF THE SITUATION 

One of ttie drug ddivay routes fr<»n Scnidi ami Centrd Ametica to the Utvtcd 
is ccmducted by sea. Drugs are very often carrkd by ine»is ot ft@t boats ftiat operaie 
between these points. These boats ate uwally equif^jed with coxmmonMatiofl ^[UipEtmts 
and often with riuiar equipments whidi are used not m a ^>ecific and ^«dard opendioiial 
mode but with random trananitting. 

B. SCENARIO 

We will assiune that a fast patrol boat t ransmi t tii ^ between ^^kms and equipped 
with radiu equijnnait, is bdi^ operated without fcdiowing a spedfic and standard 
transmisdrm {dans. Two strsd^cally-dtuated anall |>atrol boats are gcdng to oi}erate in a 
{passive way sudi that th^ do md reveal their {>osition. These boats Mrill use Direction 
Finding (DF) equipments hr tl« radar bamls, whidt will fnovide bsnit^ of the received 
emissiems and whidK will be used to determine the drug boat {Tosibon from fttese two 
t'earii^. It will tradk the boat in order to detennir« the possible future dtestinatfon of die 
ddivery. 

The tracing of the target ^dll be coruiucted by usiz^ the Kalman Riier algc»ftlum. 

C. OBJECTIVE OF THIS REPORT 

This refTort will aiial 3 rze some DF equipmotts availabte in the xn^ket whidt mi^ 
be instaiied or. b^d patrol boats, in order to determine tl^ ^curacy of dte ttaddi^ 
using bearings only. 
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Input <misklaati(Mns %v01 be ttie avaflaUe data <rf DF ^nuaqr tv etiOT of tlw ^rstene 
as oHttned from 1. Hus will depend tm the baiul operatton of ttie eqpnpmait 

aiKl die ptupt^ frv whidt it is intended (radv or coDnmui^tkmi diiectk»n findr^. 

Fast drug bmUs ncvmally travd at a comtaitt ^>eed ai^ coui% betit^en Ae 
destination pmts d the cMivertes. For Ae ptnpose of Ais malyds, Ae target is i^sumed 
to have a coi^nt ^peed during Ae detectbn aiKl detenninalNn of Ae beata^ phase, ttKl 
%vin indude fri some cases a duu^ of course ttdti A will afried Ae axuracy of tiu baddi^ 
]Voce^. 
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II. FORMULATION OF THE niOBLEM 


Tlw protein is based on the detects of the traimn^ons by d^rndnii^ dtt 


directi<m of ttte enus^ms. 

This »tuation is shown in Rgure 1. 



The Kalman filter algorithm wiU process the leceved bearii^ frc«n the two patnrf 
boats and will ^timate the tai^et position l«e|nng of its trajectory. 
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A. THE SYSTEM MODEL 


The hv'o patrol boats will use a reference system for position determuiation based on 
X and Y <XK>rdinate. The position of the patrol boats is known, as is dtdr course and 
speed. The d^iied infonnation about the drug boat is its posititm and vdod^ in X and 
Y coOTdinato. 

The coordinate portion is based on Ure folkwing d 3 mainks: 




( 2 - 1 ) 


where ; 




( 2 - 2 ) 


• X : Actual comviinate in X 

• y : Actual coordinate in Y 

• y^.,: Previous portion in X 

• yj.,: Previous position in Y 

• X ; Velocity component in the X direction 

• >• : Vdodty con^pownt in the Y direction 

• T ; Hme mtCTval of the observation, which refuesetts the tiiw since previous 
positions were iwasured. 

In order to e^imate these paran«tCTS, we need to have a model for tl« ^st^. In 
this case, the discrete state-space model representation is given by Equaticm (2-3) w'hidt is 
a standard state space matrix rqn’esottation of a linear sv^m <rf discrete diffsence 
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(2-3) 



B. THE MEASUREMENT MODEL 

TIk eqialdora that involve a non-finw mois^aneineM process are relMed to die sUMe 
vanables and am be nK>de)ed the hrflowii^ equirtkm : 

where ; 

z = Measured data (input to the sv^arO 
X = Sate vector 
V Meawranent Noise 

TIk me^urttdenl data involve the bearii^ c^itau^ frKn the DF eqiupnioMs «m 
board the two patrol boats as stated in Rgure 1. This represe nts a iwn biau- rel^tomlup 







t ’I ■ the measurements and the state variables. The actual measurement equation is as 

follows : 


2*=tan-'' 






+V. 


(2-7) 


where : 


Zk 

= Observed true bearings from patrol boats 

Xk >yk 

= Position of the drug boat at time k 

Xnk »ynk 

= Position of patrol boat "n" at time k 

Vk 

= Measurement noise 


This measmement equation should now be linearized in order to be able to work 
with the linear Kalman filter equations. 

C. NOISE CONSIDERATIONS 

We will consider the error associated with the bearing accuracy of the DF 
equipments. 

The measurement noise v will be assumed as having zero mean and variance given 
by the specifications stated in Reference 1. This will constitute the measurement error 
source. 

The bearing accuracy from the specific equipments installed on board the patrol 


boats were verified. 









III. KALMAN HLTER 


The application of the principles of modem estimation techniques to multisensor 
navigation systems began shortly after they were published [Ref. 3]. The Kalman filter 
presented "a technique for systematically employing all available external measurements, 
regardless of their errors, to improve the accuracy of navigation systems". (Ref.3: p.5] 
The filtering process refers to the estimation of the state vector at the present time 
based upon present and past measurements. In order to perform its job, the filter needs an 
a ^•'riori knowledge of the state estimate # its error covariance matrix and 

also the actual observation z ^^, which in the case of this report, represents the bearings 
obtained by the DF equipments on board the patrol boats. 

Figure 2 shows the entire process to estimate the state of a linear system composed 
of the "System", "Measurement" and "Kalman Filter" with the sources of errors considered: 
System error and Measurement error. 
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SYSTEM MEASUREMENT A PRIORI 

ERROR ERROR INFORMATION 

SOURCES SOURCES ■ 




SYSTEM 






SYSTEM 

STATE 

CVCTPM 

STATE 

— N 

MEASUREMENT 

OBSERVATION 

\ 

KALMAN 

ESniiUTE 

LZ 


x(t) 

z(t) ' 

FILTER 

x(t) 


Figure 2. Block diagram depicting system, measurement and estimator 


A. EXTENDED KALMAN HLTER 

The measur* • equation formulated for the present case (Eq. 2-7) represents, a 
nonlinear relationsh ' ^'tween the observed bearings and the state variables. In order to 
use the Kalman filter eq... tions in this situation, it has to be "adapted" to this nonlinear 
application. This adaptation of the Kalman filter to a nonlinear application constitutes the 
Extended Kalman Filter. 

1. Linearization 

For the linearization process, the Jacobian of the nonlinear measurement 
equation, has to be formed, so ; 

(3-1) 

where the observation matrix h^ is a hmction of the state at each sampling time. 

In order to linearize this equation we need to expand h in a Taylor series expansion 
about a estimated trajectory which is continually updated with the filter's estimates. 
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In this fashion we obtain a first order approximation keeping only the first term in 
the series expansion. 


Taking the Jacobian of Equation 2-7, 





(3-2) 


and applying the lineari 2 ation method, we get 


2 

tan'l 



^k-yJ 


5^* 


which by simplification gives 


where : 


^*■[^11 ^12 ^13 ^14 




fi 

tan'l 






_^k-y„i) 


bx, 

£ 


h^2-- 


t, 

tan'l 



(yk-yJ 




=0 


$ 

tan'l 




(y*-7j 

_ (**-^«*) 


57* 

£ 


(3-3) 


(3-4) 


(3-5) 


(3-6) 


(3-7) 
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Once the measurement equation is linearized about which represents the 

updated state estimate at time k, based on the previous estimates calculated at time K-1, 
the normal linear Kalman filter equations can be applied to solve the problem. 

2. Noise Processes 

In order to compute the error covariance matrix, the filter needs to know the 
covariance matrix of the measurement noise process Vj,. 

£|v.v/l=i!. (Ml) 


10 








where R,, is defined as the state measurement noise covariance matrix, and it is based on 
the accuracy of the DF equipments. 

The excitation covariance matrix Qk, involves the stochastic model of 
accelerations of the target. 

The geometry of the target will be represented as specified in the Figure 3, 



X 

Figure 3. Geometry of the Target 

where it can be seen that the velocity of the target can be described as: 


K,=Ksm(e) 

(3-12) 

Kj,=Kcos(0) 

(3-13) 


By taking the time derivative of these two equarions we get the target's 
acceleraHon in its x and y components 
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a^=7sm(e) *-l^cos(6) 


■K K 

-.y-I + yQ-Z 

y V 


(3-14) 


■K ■ 

=F-i+eK 

y y 


a^- Kcos(0) - VBsiii(0) 


.y .y 
=v-^~yB— 
y y 


(3-15) 


■K ■ 

=K-Z-eK 

y * 


Assuming a linear and angular accelerations having a zero mean 


£[l^=£[6]=0 


the variances becomes 


(3-16) 




(3-17) 


4el-o5 

The assumed values for the linear and angular accelerations were taken as: 


ol=0.0«)5r■■ 


(3-19) 


Of 

hr^ 


( 3 - 20 ) 






In order to calculate the state excitation covariance matrix Q,^, we need to have 
the variances of the accelerations in the x and y directions, so taking the expected value 
of the Equations (3-14) and (3-15) we will have : 



(3-21) 





(3-22) 


and the covariance of a^ and is equal tv-: 






(3-23) 


With all these calculations the value of the state excitation covariance matrix 


becomes ; 


where is the system noise coefficient matrix and is represented by : 


(3-24) 


and Q' is equal . 



0 

0 

H 

2 

T 


'^o'] 

'*[s) AA. 


(3-25) 


(3-26) 
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3. Initialization of The Extended Kalman Filter 


For the purpose of obtaining the best and accurate estimates from the Kalman 
filter process, the filter must be initialized with an Initial State Estimate and an Initial Error 
Covariance matrix. If this initial state estimate is far from the real target position, the 
linearization effects from the nonlinear measurement equation will cause the filter to 
diverge, giving an erroneous output. 

The initialization of the filter will be based on the intersection of the first two 
received bearings. The estimated initial target position will be calculated as : 



y^^tanCfi;) +y^;tan(e.) 
tanffijl-tanffij) 




(3-27) 


y^,tan(e,)^y,,tan(e,)^x^3-x^; (3-28) 

tan( 0 ,)-tan( 02 ) 

Since no information is available on the course and speed of the target at the 
initialization moment that Ccold help in the estimation of the velocities of the target ii'. .X 
and Y directions, they are taken as zero. Figure 4 represents the initializalion of the initial 
position estimate. 

This initial position estimate and the assumption of zero velocity in the x and y 
directions include some error that can be assumed to be within some standard deviation. 
The estimate of the errors of this initial estimates will be used to construct the Initial Error 
Covariance matrix. 


Following the approach of Bermett (Ret. 4J and Galinis [Ref.5], the value of an initial 
standard deviation of position error is taken as 100 nautical miles both in the x and y 
directions, and the initial velocity standard deviation is taken to be 0.5 nautical miles per 







DRUG'S BOAT POSinoK 


Initial Fonltion 

Estimate C*T~> True Poeition 

CZX> 


X ■ ' 


Observed Bearings . 


Ti-ue Bearings 


! 01 / /' 



PATROL BOAT 1 


PATROL BOAT Z 


Figure 4. Initialization Process 

minute which is equal to a 30 knots speed. Both errors are aswsumed to be tmcorrelated and 
have zero mean. 

The resultant Initial Error Covariance matrix for time k=0, can be written as : 


P(A/fc-l)=P(0/-l)= 


ioooo 0 0 0 

0 0 25 0 0 

0 0 10000 0 

0 0 0 0.25 


(3-29) 


Now that the filter is initialized the estimation process of the target position is ready 


to begin. 
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4. Extended Kalman Filter Operation 


For the continuous operation of the filter, the observation bearings are 
simultaneously and continuously received fi-om the two sensors at a time interval T equal 
to the time difference in minutes between the observations. 

The A Priori State Estimate and the State Error Covariance Matrix Pdj/n) 
are computed by using the following Equations : 






Hk 


(3-30) 


Once the A priori or Projected State Estimate is calculated, it is used to compute 
the Linearized Observation Matrix H|, in Eq.(3-9), and with all these together the Kalman 
Gain matrix is computed as follows: 




-1 


(3-32) 


The Kalman Gain matrix represents the confidence given by the filter to a priori 
information with respect to the ciarent observation. It minimizes the square estimation 
error and indicates how much weight will be placed on the current observation. If P,v;/k.i) 
is relatively small, the Kalman Gain matrix will be close to zero due to the finite value of 
R^. If P(k/k.i) is relatively large the Kalman Gain matrix will be close to one, so the Kalman 
Gain is proportional to the uncertainty in the estimate P(k/i(.i, and inversely proportional 
to the measurement noise R,,. 

The Kalman Gain matrix will directly eiffect the calculation of the state estimate 
as it is seen in Eq.(3-17) 
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v/here the Kalman Gain matrix affects the weight placed on the current obser\'ation Zj,. "A 
large Gain, indicatLig a large error covariance, wili place more weight on the current 
observation as the filter tries to correct the states. A small gain, indicating a small error 
covariance, places less emphasis in the new observation" [Ref.5: p.l5]. 

The Error covariance matrix will also be updated by considering the Kalman 
Gaiir matrix as it is stated in the next equation: 


The process then will repeat itself by computing the next observation from the 
two sensors, and the continuous estimation process of this information will allow the filto" 
to keep track of the target movements and maneuvers. 


B. MANEUVER DETECTION 

For the present work, the target is assumed to have a constant course and speed for 
the initial scenario and then it is included to have a change of course in order to ev'aluate 
•h-. response and the ability of the filter to keep the tracking process for the evaluation of 
. (..h independent DF system. 

In order to be able to keep track of the target once a maneuver has occurred, the 
fitter ha-s to use some kind of detection system. 

The use of the residual bias as a maneuver detector, as proposed by McAulay and 
Denlinger [Ref. 6] and followed by Bennett [Ref.4], will be used in the present thesis by 
applying a second-erder moving average filter as the residual bias. 

The output of the bias filter is: 
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(3-35) 


Ir 




where the observation residual Ct is defined as : 


The window provided by the moving average filter is wide enough to absorb a large 
amotmt of error in the bearing observations that are ftur outside the standard deviation, but 
narrow enough to detect a maneuver as soon as it has actually occurred. 

When the mean of the residual process exceeds a maneuver detection threshold, the 
filter determines that a maneuver has occurred and resets the filter parameters in order to 
maintain an accurate track imtil the following maneuver is detected. This detection 
threshold or GATE is chosen to be 1.5 times the standard deviation of the residual process 




(3-37) 


which as stated in Reference 6 will result in a 90% probability of detection of the maneuver 
with a 10% probability of false alarm rate. 
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IV. DRUG SMUGGLERS AT SEA 


Die drug traffic at sea is conducted using boats and electronic equipment available 
in the rommerdal area. The military services have been tasked to coimts- tl«se 
organizations. 

The US Department of Defense ^ablished two control centers for drug interdiction 
in the United States. In February 1989 the Joint Task Force 4 was ^ablished in Key VV^, 
Florida and Joint Task Force 5 in Alameda, California, each of them having the 
responsibility of anti-drug operations in ther respective opoational areas, Caribbean and 
Pacific. 

The boats and ships used for the drug ddivery purposes are commerdal and 
recreational and are equipped with navigation radars available anywhere in the market. 
Those belonging to the FURUNO company are the mcKt popular. This radar will be used 
on the present thesis with the characteristics and informaticm avail^le frwn the FURUNO 
product catalog (Ref. 7J. 

A. RADAR CRARACTHUSDCS 

The radars installed could be one of several available from this com|»ny 
(1700.1«)0,1900, FR-7Q00D,FR-8000D FR-1500D,FR-2000 or CR-900) whidi operate in the X 
and S bands at diffe'ent output powers for differ«it range. 

We will assume that the equipment bdongs to the FR-8000D Series from 

FURUNO (modd FR-8100K), whjch has the following characterises, as it is stablid^ 
in Reference 7: 
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• Navigation radar 

• S-band operation (2-4 Ghz.) 

• High resolution : 640 x 480 pixel 12" CRT display 

• High accuracy ; 0.9 % of range in use and 0.2 degree bearing resolution 

• 10 Kw of output power 

• No compromise 8-level quantization, coupled with a MIC low noise receiver, four 
transmitter pulselengths, three pulse repetition rates and two receiver IF bandwidths 

• Range ; 1 /4 to 72 nautical miles 

• Improved detection by Echo Stretch, Echo Averaging, Interference Rejector, Sea & 
Rain Clutter Control 
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V. DIRECTION FINDING EQUIPMENTS TO BE ANALYZED 


The selected DF equipments will be considered to evaluate the performance of the 
estimation process for each equipment. The characteristics of them will be specified as 
follows as taken from Jane's [Ref. l:pp. 346-352]. 


A. GUARDIAN STAR SHIPBORNE EW SYSTEM 

• Shipbome radar detection and surveillance system 

• Frequency range ; 2-18 GHz 

• The system consists basically of an antenna assembly with an onmi-directional and 
six spiral DF antennas, an RF/digital interface imit and a display/controller uiut 

• Versions : The MK 1 system offers early warning to small surface craft or patrol 
boats with a bearing acciuacy provided by octave frequency measurements. The MK 
2 system is designed to meet the basic ESM needs of surface ships and submarines 
providing YIG tuned frequency. The MK 3 is an ELINT system for surface ships and 
submarines, providing instant threat wanring and accurate frequency measurement 
by IFM devices in the receiver front end 

• Can store up to 2000 emitter parameters in the library 

• DF accuracy ; 2.5 degrees in the 2-8 GHi' h^nd 

1.5 degress in the 8-18 GHz btu.o 

• Operational status ; In production 

• Contractor : Sperry Corporation, Arlington, Virginia, USA 


B. ELETTRONICA SpA. EW EQUIPMENT 

• Shipbome integrated EW system 

• Frequency range : 2-18 GHz 
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• Based on a four-band instantaneous frequency measuring (IFM) receiver which 
perform all ESM functions 

• Components : ELT/116 Radar intercept receiver, ELT/711 Radar identification unit, 
ELT/712 Prograauiung unit, ELT/716 Data transmission module, ELT/311/511 
Jammer 

• The ELT/116 radar intercept receiver system includes omnidirectional and DF 
antennas, an auxiliary DF unit, RF unit and display .The IFM receiver operates wi»^h 
a crystal video DF receiver in all bands 

• Frequency measurement accuracy : 0.2 % with a Dynamic range of about -70 c 8m 
to zero 

• DF accuracy : 7 degrees 

• Operational status : In production and in operational use 

• Contractor : Elettronica SpA, Rome, Italy 


C. RDL-IBC ESM EQUIPMENT 

• Shipbome radar surveillance and detection system 

• Frequency range : 2-11.5 GHz 

• The system is a basic tactical small ship ESM system, suitable to use on boaid fast 
patrol boats 

• Provides instantaneous bearing, automatic pulse analysis (APA-IC Puise Analyzer) 
and alarm together with measiuement of frequency band 

• DF accuracy : 20 degrees 

• Operational status : In service but no longer in production 

• Contractor : Racal Radar Defence Systems Ltd. Chessington, England 
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Vi. SIMULATIONS 


ITie vrograms here are based on thc:;e dex'elop^ by Spehn [Ref. 2], Bennett [Ref. 4] 
and Galinis [Ra. 5] which are written in Microsoft FORTEtAIs', ?jnd MaTLAB. The 
following especifications should be followed ^v■hen desired to run. 

o TRKDATA.FOR : Will generate the data needed for the calcu’ation and estimation 
process, having the opt. < of using noisy or no-noise bearing observation^ from two 
observers. In order to run the program in the noisy option, a file must be generated 
containing the noise from a random number generator which should consider the 
variance of the error associa‘-ed u\ this case vrith the accuracy of the ESM 
•ystems. The output of this pi gram is stored in the file TRKDATA containing the 
TIME, X and Y coordinates o' ;ne sensors and the Bearings from each sensor. This 
file is needed to run the main program SHIPTRACK.FOR. 

• SHIPTRACK.FOR; Will read the file TRXDATA and based on the Extended Kalman 
filter equations, will calculate the estimated position of target, producing the 
following output files : OUTDATA which contains the time, estimated X and Y 
coordinates of the target, calculated X and Y coordinates of the target based on the 
bearings measurements from sensors 1 and 2. TRKERR containing the time, tracking 
and observed errors, and TRKINFO which contams the lime, and the estimated 
parameters of the target: X and Y coordinates, course and speed. 

• PLOT.M ; A MATLAB function file created to graph the results obtained from the 
shnulatious. For this purpose, the output files should be transferred to a MATLAB 
subdirectory that already conteiins this function file. 


A. DESCPaPTION OF THE SCENARIOS 

In order to see the effects of the tracking process by ti.e Kalnion filcer algorithm, 
three different scenarios will be considered. The drug smugglers boat is transmitting 
between its delivery stations and is being monitored by two patrol boats which obtain 
bearings from the transmission of the radar installed on board llie drug boat. 
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For all simularions, the patrol boat travels at «he s" ('oiirse and at a speed of 16 
knots <a very re39‘>nabie cruisiiv’^ jpeed for a ship), so that no suspidor' is produced. 

Each situation wiii De ajulyzed -Aath the three DF eq\uptnent5 previously described 
;u v_napter V. 'Fhe performance of each of them will be evaluated. 

Three scenario- 'jiown v/jth th'*. following characteristics; 

1. Scenario Nr.l 

• Cnig boat traveling at course 090 degrees at .30 knots 
Patrol t*.' Jt 1 travelir^ a • course 060 at 16 knots 

• Patrol boat.raveling at cou'se 350 at 16 knots 

• scenario is shown in Figure 5 

2. Scenario Nr. 2 

• Drug boat traveling at course 090 degrees at 30 knots until time t=15 minutes when 
it maneuvers assuming course 045 maintaining a speed of 30 knots 

• Patrol boat 1 traveling at course 060 at 16 knots 

• Patrol boat 2 traveling at course 350 at 16 knots 

• This scenario is shown in Figure 6 

3. Scenario Nr. 3 

• Drug boat traveling at course 090 degrees at 30 knots until time t=15 minutes when 
it maneuvers assuming course 000 maintaining a speed of 30 knots 

• Patrol boat 1 traveling at course 060 at 16 knots 

• Patrol boat 2 traveling at course 350 at 16 knoto 

• This scenario is shown in Figure 7 
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Figure 6. Scenario Nr. 2 
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Figure 7. Scenario Nr. 3 
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B. RESULTS OF THE SIMULATIONS 


1. GUARDIAN STAR SHIPBORNE EW SYSTEM 


a. Scenario Nr 1. 


With a DF accuracy of 2.5 degrees, the filter will keep following the target 
movements very dose to its real positions. The maximum tracking error is 0.32 nm 
maintaining an average of 0.14 nm The results of this scenario are shown in Figures 8 and 


9, and the output file TRKINFO containing the target's estimated data is shown below. 


TIME(min) 

X POS(nm) 

Y POS(nm) 

HDG(deg) 

SPD(knots) 

0 

5.378387 

14.905070 

0.00000 

0.00000 

1 

5.763358 

14.938050 

88.170590 

13.833230 

2 

6.526980 

15.073220 

83.468400 

31.097350 

3 

7.145387 

15.114900 

84.569790 

33.608130 

4 

7.309046 

14.965760 

91.372160 

25.715370 

5 

7.934094 

15.007730 

89.623790 

28.892010 

6 

8.546397 

15.039240 

88.955980 

30.698860 

7 

8.781705 

14.955550 

91.143860 

27.387130 

8 

9.302892 

14.962130 

90.782200 

28.072280 

9 

9.900760 

14.988120 

90.143300 

29.308670 

10 

10.361610 

14.984620 

90.179310 

29.069630 

11 

10.865720 

14.990180 

90.058300 

29.224970 

12 

11.338580 

14.996020 

89.951840 

29.120680 

13 

11.678660 

15.012880 

89.684380 

28.131570 

14 

12.339770 

15.018430 

89.622230 

29.356790 

15 

12.848020 

15.027450 

89.532430 

29.471050 

16 

13.327280 

15.045920 

89.329100 

29.405160 

17 

13.819690 

15.063880 

89.154300 

29.419710 

18 

14.411500 

15.026960 

89.685640 

29.935990 

19 

15.003690 

14.962570 

90.491620 

30.390950 

20 

15.475100 

15.005990 

89.897880 

30.227580 

21 

15.960590 

15.042060 

89.452220 

30.147910 

22 

16.417440 

15.162740 

87.958690 

29.971100 

23 

16.945640 

15.091350 

89.122680 

30.075770 

24 

17.465870 

14.986290 

90.604700 

30.147870 

25 

17.973440 

14.835460 

92.532780 

30.190030 

26 

18.481840 

14.931920 

90.927700 

30.189910 

27 

19.010840 

15.152880 

87.744030 

30.301360 

28 

19.519140 

15.189950 

87.507160 

30.318730 

29 

20.011280 

15.137270 

88.628120 

30.259360 


28 








TRACKING PROCESS 



Figure 8. Target tracking 
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b. Scenario Nr 2. 


For this scenario, the filter will follow the target maneuver to course 045 
degrees by evaluating the next 7 observations after the maneuver is produced. This induces 
a tracking error that reaches 0.94 nm after the maneuver takes place, which is reduced to 
about 0.25 nm when the maneuver gating of the filter corrects the estimation process. The 
results of this scenario are shown in Figures 10 and 11, and the output file TRKINFO 
containing the target's estimated data is presented below. 


TIME(min) 

0 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 
11 
12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 
29 


X POS(nm) 

5.378387 

5.763358 

6.526980 

7.145387 

7.309046 

7.934094 

8.546397 

8.781705 

9.302892 

9.900760 

10.361610 

10.865720 

11.338580 

11.678660 

12.339770 

12.810740 

13.223690 

13.625840 

14.106980 

14.577360 

14.921810 

14.836720 

15.041560 

15.608130 

16.114400 

16.603260 

16.884370 

17.090610 

17.420320 

17.805230 


Y POS(nm) 

14.905070 

14.938050 

15.073220 

15.114900 

14.965760 

15.007730 

15.039240 

14.955550 

14.962130 

14.988120 

14.984620 

14.990180 

14.996020 

15.012880 

15.018430 

15.116240 

15.284670 

15.496370 

15.693230 

15.902150 

16.231590 

17.593810 

18.031290 

18.298390 

18.513630 

18.675180 

19.226940 

19.950330 

20.380020 

20.662320 


HDG(deg) 

0.000000 

88.170590 

83.468400 

84.569790 

91.372160 

89.623790 

88.955980 

91.143860 

90.782200 

9C.143300 

90.179310 

90.058300 

89.951840 

89.684380 

89.622230 

88.227130 

85.992000 

83.402250 

81.397750 

79.511890 

76.159880 

47.291450 

31.958180 

50.397030 

56.217260 

60.262940 

51.627830 

42.344990 

41.441830 

43.124460 


SPDOcnots) 

0.000000 

13.833230 

31.097350 

33.608130 

25.715370 

28.892010 

30.698860 

27.387130 

28.072280 

29.308670 

29.069630 

29.224970 

29.120680 

28.131570 

29.356790 

29.262770 

28.902960 

28.610620 

28.798490 

28.962680 

28.764900 

31.042000 

27.265090 

31.575780 

31.943490 

31.517530 

31.718100 

33.174030 

33.107540 

32.322650 
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Figure 11. Observed and tracking errore 
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c. Scenario Nr 3. 


With a DF accuracy of 2.5 degrees, the filter will evaluate three more 


observations before detecting the target maneuver. The tracking error reaches 1.05 nm 
while the maneuver is not detected it is reduced to about 0.1 nm by the maneuver gating 
after it detects the change of course, keeping a close track of the target movements. The 


results for this scenario are shown in Figures 12 and 13, and the output file TRKINFO 
containing the target's estimated data is presented below. 


TIME(min) 

0 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 
11 
12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 
29 


X POS(nm) 

5.378387 

5.763358 

6.526980 

7.145387 

7.309046 

7.934094 

8.546397 

8.781705 

9.302892 

9.900760 

10..361610 

10.865720 

11.338580 

11.678660 

12.339770 

12.718770 

12.976850 

12.290310 

12.562290 

12.832930 

12.607800 

12.490300 

12.266730 

12.369590 

12.478760 

12.623070 

12.558350 

12.369090 

12.319710 

12.347500 


Y POS(nm) 

14.905070 
14.938050 
15.073220 
15.114900 
14.965760 
15.007730 
15.039240 
14.955550 
14.962130 
14.988120 
14.984620 
14.990180 
14.996020 
15.012680 
15.018430 
15.148420 
25.38')340 
16.546470 
17.020110 
17.464880 
18.005310 
18.509500 
19.a33420 
19.539770 
20.025980 
20.494540 
21.007620 
21.540510 
22.043870 
22.550470 


HDG(deg) 

0.000000 
88.170590 
83.468400 
84.569790 
n.372160 
89.623790 
88.955980 
91.143860 
90.782200 
90.1-*3300 
90.179310 
90.058300 
89.951840 
89.684380 
89.622230 
87.738750 
84.482090 
2.415755 
22.479660 
26.589940 
7.791852 
1.641888 
355.196900 
358.533500 
0.982643 
3.427758 
1.898402 
358.897300 
358.367900 
358.887900 


SPD(knots) 

0.000000 

13.833230 

31.097350 

33.608130 

25.715370 

28.892010 

30.698860 

27.387130 

28.072280 

29.308670 

29.069630 

29,224970 

29.120680 

28.131570 

29.356790 

28.720650 

27.586280 

29.750290 

31.960900 

31.743030 

29.901250 

29.800140 

30.262980 

30.214490 

30.049860 

29.837520 

2.y.f'H280 

30.U59630 

30.058670 

30.127010 
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Figure 12. Target tracking 
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Figure 13. Observed and tracking errors 
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2. ELETTRONICA SpA. EW EQUIPMENT 


a. Scenario Nr 1. 


With a DF accuracy of 7 degrees, the observations obtained from this EW 
equipment allow the filter to follow the target's real trajectory in a very close range having 


an average tracking error of 0.25 nautical miles throughout the process. The results for this 
scenario are shown in Figures 14 and 15, and the output file TRKINFO containing the 


target's estimated data is presented below. 


TIME(min) 

X POS(nm) 

Y POS(nm) 

HDG(deg) 

SPD(knots) 

0 

5.297562 

14.839560 

0.000000 

0.000000 

1 

5.606289 

14.889010 

87.872260 

3.000554 

2 

6.251655 

15.076120 

84.202790 

13.723970 

3 

6.878047 

15.129''30 

85.245830 

21.320830 

4 

6.996462 

14.959290 

92.128600 

17.180810 

5 

7.691809 

15.012230 

89.670380 

23.334330 

6 

8.393325 

15.057860 

88.702640 

27.468300 

7 

8.522616 

14.936160 

91.944220 

23.643440 

8 

9.073889 

14.943670 

91.379620 

25.277470 

9 

9.745502 

14.987170 

90.270650 

27.624500 

10 

10.196570 

14.979060 

90.378960 

27.544940 

11 

10.714290 

14.987440 

90.189420 

28.005710 

12 

11.179040 

14.995240 

90.040020 

27.990760 

13 

11.423670 

15.014810 

89.716520 

26.488830 

14 

12.195530 

15.038620 

89.425810 

28.582370 

15 

12.714280 

15.051680 

89.317120 

28.835680 

16 

13.184210 

15.078110 

89.059140 

28.778160 

17 

13.675140 

15.104270 

88.839810 

28.841620 

18 

14.332260 

15.052760 

89.486910 

29.734760 

19 

14.990810 

14.957260 

90.464160 

30.525880 

20 

15.445360 

15.017140 

89.851720 

30.273840 

21 

15.922470 

15.064400 

89.418130 

30.153260 

22 

16.347350 

15.223530 

87.984070 

29.836090 

23 

16.897680 

15.133180 

88.968120 

30.046650 

24 

17.439610 

14.998850 

90.238110 

30.210470 

25 

17.966550 

14.798780 

91.953250 

30.322900 

26 

18.470410 

14.911470 

90.837410 

30.299770 

27 

18.995870 

15.192560 

88.360760 

30.368650 

28 

19.508590 

15.249040 

88.002770 

30.395650 

29 

20.001910 

15.191520 

88.658020 

30.348060 


37 




















^n \n 


(sajiui iraijncu) ^OHH3 


Figure 15. Observation and tracking errors 










b. Scenario Nr 2. 


In this case, the bearings provided by the ESM system with a DF accuracy 
of 7 degrees, will not allow the filter to use the maneuver gating in order to detect the 
target's maneuver. After the maneuver takes place, the tracking error increases to 1.35 nm 
diminishing to 0.85 at the end of the process. The results for this scenario are shown in 


Figures 16 and 17, and the output file TRKINFO containing the estimated target's data is 


presented below. 


TIME(min) 

0 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 
11 
12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 
29 


X POSfnm) 

5.297562 

5.606289 

6.251655 

6.878047 

6.996462 

7.691809 

8.393325 

8.522616 

9.073889 

9.745502 

10.196570 

10.714290 

11.179040 

11.423670 

12.195530 

12.676740 

13.079730 

13.478650 

14.021290 

14.554630 

14.878760 

15.212270 

15.473110 

15.870110 

16.274770 

16.698180 

17.037490 

17.338860 

17o97270 

18.087560 


Y POS(nm) 

14.839560 

14.889010 

15.076120 

15.129430 

14.959290 

15.012230 

15.057860 

14.936160 

14.943670 

14,987170 

14.979060 

14.987440 

14.995240 

15.014810 

15.038620 

15.133140 

15.296360 

15.497200 

15.657320 

15.811330 

16.117730 

16.431530 

16.844350 

17.104770 

17.358180 

17.576720 

18.017270 

18.635320 

19.110920 

19.504630 


HDG(deg) 

0.000000 

87.872260 

84.202790 

85.245830 

92.128600 

89.670380 

88.702640 

91.944220 

91.379620 

90.270650 

90.378960 

90.189420 

90.040020 

89.716520 

89.425810 

88.241260 

86.339990 

84.218650 

82.972950 

81.983630 

79.330950 

76.872010 

73.551480 

72.254100 

71.178310 

70.557170 

67.980550 

64.223680 

62.089860 

60.839600 


SPD(knots) 

0.000000 

3.000554 

13.723970 

21.320830 

17.180810 

23.334330 

27.468300 

23.643440 

25.277470 

27.624500 

27.544940 

28.005710 

27.990760 

26.488830 

28.582370 

28.624480 

28.256310 

27.967140 

28.445910 

28.826030 

28.344940 

28.021690 

27.615560 

27.633100 

27.681020 

27.745740 

27.910070 

28.363990 

28.751550 

29.052230 
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Figure 16. Target tracking 


X coordiiiale (Naulical Miles) 












Figure 17. Observation and tracking errors 









c. Scenario Nr 3. 


The 90 degrees maneuver is detected by the filter's maneuver gating by 
evaluating eight (8) observations after the maneuver took place at 15 minutes. While the 
maneuver was not detected, the tracking error reaches a value of 1.92 nm which is later 
decreased to 0.95 nm. The results for this scenario are shown in F’gtires 18 and 19, and the 


output file TRKINFO containing the target's estimated data is presented below. 


TIME(min) 

X POS(nm) 

YPOS(nm) 

HDG(deg) 

SPD(knots) 

0 

5.297562 

14.839560 

0.000000 

0.000000 

1 

5.606289 

14.889010 

87.872260 

3.000554 

2 

6.251655 

15.076120 

84.202790 

13.723970 

3 

6.878047 

15.129430 

85.245830 

21.320830 

4 

6.996462 

14.959290 

92.128600 

17.180810 

5 

7.691809 

15.012230 

89.670380 

23.334330 

6 

8.393325 

15.057860 

88.702640 

27.468300 

7 

8.522616 

14.936160 

91.944220 

23.643440 

8 

9.073889 

14.943670 

91.379620 

25.277470 

9 

9.745502 

14.987170 

90.270650 

27.624500 

10 

10.196570 

14.979060 

90.378960 

27.544940 

11 

10.714290 

14.987440 

90.189420 

28.005710 

12 

11.179040 

14.995240 

90.040020 

27.990760 

13 

11.423670 

15.014810 

89.716520 

26.488830 

14 

12.195530 

15.038620 

89.425810 

28.582370 

15 

12.585460 

15.160430 

87.851670 

28.087250 

16 

12.833990 

15.378470 

85.125050 

26.935080 

17 

13.034070 

15.654520 

81.796400 

25.789000 

18 

13.345960 

15.922200 

79.019260 

25.423120 

19 

13.632530 

16.215840 

76.187030 

25.068460 

20 

13.699830 

16.628960 

71.648550 

24.019530 

21 

13.765090 

17.047440 

67.284070 

23.231020 

22 

11.636970 

19.155960 

315.930000 

49.322520 

23 

11.460090 

19.848020 

323.451900 

45.293120 

24 

11.590990 

20.522890 

338.381500 

40.542750 

25 

12.019920 

21.131820 

355.700000 

38.295550 

26 

12.052590 

21.684220 

357.379900 

37.427800 

27 

11.747040 

22.235710 

351.789100 

36.885060 

28 

11.753030 

22.768380 

353.236700 

36.140730 

29 

11.923520 

23.291300 

356.873500 

35.452060 
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Figure 18. Target tracking 


X coordinalc (Navilical Miles) 





















3. RDL-IBC ESM EQUIPMENT 


a. Scenario Nr 1. 

Although the DF accuracy for the current ESM system is very high and 
the observation error reaches a value of 4.2 nautical miles at 27 nunutes, the filter is 
capable to follow the target's trajectory by keeping an average tracking oror of 0.60 mn. 
The results for this scenario are shown in figures 20 and 21, and the output file TRKINFO 
containing the target's estimated data is presented below. 


TIME(min) 

0 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 
11 
12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 
29 


X POS(nm) 

5.160920 

5.468747 

6.084392 

6548177 

6525669 

7.084311 

7.743289 

7.809889 

8.364488 

9.107235 

9575869 

10.138350 

10.624040 

10769660 

11.718900 

12.280000 

12.761050 

13272120 

14.057480 

14.851450 

15.298(S0 

15.776880 

16.156480 

16.755520 

17541010 

17.899970 

18595630 

18.913200 

19.425000 

19.914740 


Y POS{nm) 

14.724770 

14.800420 

15.106420 

15.190230 

14.965950 

15.013260 

15.115360 

14.951200 

14.962200 

15.031990 

15.015540 

15.029510 

15.036990 

15.030270 

15.123080 

15.145620 

15.179520 

15.216480 

15.164190 

15.028410 

15.095960 

15.145780 

15.358590 

15.213640 

14.991460 

14.659020 

14.819460 

15.233950 

15.317550 

15.235900 


HDG(deg) 

0.000000 
86.873050 
83 274390 
84.770480 
92359770 
88.849180 
87.670190 
91.927350 
91.087980 
89.456180 
89.833470 
89.635210 
89568810 
89.698510 
88.474850 
88364200 
88.108360 
87.85686U 
88.628620 
90.027480 
89.409730 
89.010560 
87.234490 
88.651090 
90502660 
92.976840 
91.609440 
88527730 
88.033440 
88.716510 


SPDOcnots) 

0.000000 

0.437V_-7 

2.865865 

5.647289 

4.606804 

9.187359 

14.094480 

12.495720 

15591290 

19.656760 

20.768670 

22.367990 

23.150570 

21586090 

25210810 

26.030660 

26.295410 

26.682810 

28374460 

29.899680 

29.665790 

29.601390 

29.148330 

29595090 

29.961640 

30.240700 

30.175070 

30.197700 

30.229680 

30.184120 
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b. Scenario Nr 2. 


The DF accuracy of 20 degrees for the crirrent ESM system giving an 
obser\'ation error with high values such as 1-5, 1.7, 1.95 and 2.4 nauticai miles, will drive 


the filter to fail in the use of the maneuver gating to detect the targi>t change of course, 
with the tracking error increasing from 0.42 to 1.62 nautical rrdles at 25 minutes. The results 


for this scenario are shown in Figures 22 and 23, and the output file TRKINFO containing 


the ta’ estimated data is presented below. 


TlME(min) 

X POS(mn) 

Y POS(nm) 

HDG(deg) 

SPD(knots) 

0 

5.160920 

14.724770 

0.000000 

0.000000 

1 

5 468747 

14.800420 

86.873050 

0.437957 

2 

6.084392 

15.106420 

83.274390 

2.865865 

3 

6.548177 

15.190230 

84.770480 

5.647289 

4 

6.525669 

14.965950 

92.359770 

4.606804 

5 

7.084311 

15.043260 

88.849180 

9.187359 

6 

7.743289 

15.115360 

87.670190 

14.094480 

7 

7.809889 

14.951200 

91.927350 

12.495720 

S 

8.364488 

14.962200 

91.087980 

15.591290 

9 

9.107235 

15.031990 

89.456180 

19.656760 

10 

9 575869 

15.015540 

89.833470 

20.768670 

11 

10.138350 

15.029510 

89.635210 

22.367990 

12 

10.624040 

15.036990 

89.568810 

23.150570 

13 

10.769660 

15.030270 

89.698510 

21.586090 

14 

11.718900 

15.123080 

88.474850 

25.210810 

15 

12.242700 

15.212580 

87.485190 

25.829440 

16 

12.656120 

15.362420 

85.835110 

25.783480 

17 

13.072130 

15.549900 

83.940970 

25.797750 

18 

13.735540 

15.686430 

83.139880 

27.027680 

19 

14.395680 

15.785930 

82.863380 

28.071040 

20 

14.706770 

16.076720 

80.412510 

27.546340 

21 

15.033500 

16.370960 

78.166890 

27.193910 

22 

15.234080 

16.799460 

74.644880 

26.545540 

23 

15.668290 

17.002680 

73.907650 

26.678530 

24 

16.114670 

17.180200 

73.498730 

26.816180 

25 

16.591350 

17.288950 

73.749410 

26.973330 

26 

16.904000 

17.716840 

71.185680 

26.903340 

27 

17.129320 

18.385210 

66.893610 

26.929530 

28 

17.449740 

18.827900 

64.814590 

27.026090 

29 

17.822280 

19.143750 

63.907220 

27.105550 
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Figure 22. Target tracking 
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Figure 23. Observed and tracking errors 
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c. Scenario Nr 3. 


In this case, the poor DF accuracy of the ESM system will not allow the 
filter to use the maneuver gating algorithm to detect the target's maneuver at 15 minutes, 
driving the tracking error from 0.42 to 2.78 nautical miles after the maneuver took place. 
The results of this scenario are shown in Figures 24 and 25, and the output file TRKINFO 
containing the target's estimated data is presented below. 


TIME(min) 

0 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 
11 
12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 
29 


X POS(nm) 

5.160920 

5.408747 

6.084392 

6.548177 

6.525669 

7.084311 

7.743289 

7.809889 

8.364488 

9.107235 

9.575869 

10.138350 

10.624040 

10.769660 

11.718900 

12.156560 

12.420440 

12.639780 

13.069830 

13.479370 

13.533460 

13.591340 

13.504770 

13.660610 

13.833940 

14.053920 

14.062400 

13.918370 

13.903470 

13.969240 


Y POS(nm) 

14.724770 

14.800420 

15.106420 

15.190230 

14.965950 

15.043260 

15.115360 

14.951200 

14.962200 

15.031990 

15.015540 

15.029510 

15.036990 

15.030270 

15.123080 

15.227600 

15.413070 

15.653360 

15.881950 

16.120920 

16.506220 

16.895810 

17.352460 

17.700180 

18.046500 

18.374690 

18.819900 

19.336330 

19.753940 

20.130380 


HDG(deg) 

0.000000 

86.873050 

83.274390 

84.770480 

92.359770 

88.849180 

87.670190 

91.927350 

91.087980 

89.456180 

89.833470 

89.635210 

89.568810 

89.698510 

88.474850 

87.244830 

84.993490 

82.140310 

80.033790 

78.075900 

73.861470 

69.760210 

64.496540 

61.726940 

59.314390 

57.546620 

53.822770 

48.872710 

45.757290 

43.677430 


SPD(knots) 

0.000000 

0.437957 

2.865865 

5.647289 

4.606804 

9.187359 

14.094480 

12.495720 

15.591290 

19.656760 

20.768670 

22.367990 

23.150570 

21.586090 

25.210810 

25.334480 

24.533650 

23.684380 

24.024470 

24.264370 

23.103980 

22.210330 

21.081140 

20.874680 

20.786':00 

20.849960 

20.537330 

20.080590 

19.871890 

19.818920 
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Figure 24. Target tracking 
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Figure 25. Obsen^ed and tracking errors 











VIL CONCLUSIONS 


The results obtained from the difterent situations involving the DF accuracy of ESM 
systems for the tracking orocess and maneuver detection of the boat, represents how 
important the information provided by the ESM system is. Its accuracy allows the Kalman 
filter to process the observations with a high probability of the estimated data being close 
to the real trajectory of the target, in the event of high DF accuracy, the received signals 
involve a high spread in bearing indication that will affect the performance of the filter 
which would fail in the detection of the target's maneuver as observed in the results 
obtained for the ELETTRONICA EW EQUIPMENT and the RDL-IBC ESW EQUIPMENT. 

'The presented scenarios were analy 2 ed in detail by running the programs with 
theoretical DF accuracies from 1 to 20 degrees, to find the exact DF value where the 
maneuver gating no longer works. This value was determined as 4.6 degrees for the second 
srenario which involves a change of course of 45 degrees and the maneuver gating stopped 
working at 9.6 degrees for the third scenario which involves a 90 degrees maneuver. 

The improvement of the tracking process as the DF accuracy increases, was also 
calculated from these simulations and the results are presented in Figure 26, where the 
degradation of the process is seen as the error of the ESM system increases. 

The atmospheric noise and other propagation factors that afreet the reedved signals 
on the ESM equipment were not considered due to the dose range between the target and 
the patrol boats for the different scenarios. However, this can be a consideration for further 
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Figure 26. Improvement of the tracking process as a function of the DF accuracy 
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analysis where the DF accuracy of the selected systems can be confronted in a long 
distance or OTH tracking situation. 
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APPENDIX A. PROGRAM TRAKDATA 


C ***TRACKDATA*** 

C THIS PROGRAM COMPUTES THE TRUE TARGET AND SENSOR POSITIONS AND 
C CORRESPONDING TARGET BEARINGS FOR USED DESIGNATED OBSERVADON 
C TIMES. 

C VARIABLE DECLARATIONS 

REALM XT(4,1 ),XS1 (4,1 ),PHI(4,4),SPDS1,HDGS1 ^PDS2,HDGS2 
REALM DT,HDGT^PDT,XS2(4,1 ),TEMP1 (4,1 ),CASE,XDIFn,YDIFn 
REALM XDIFF2,YDIFF2,N0ISE,DT0R,RT0D,BRG1 ,BRG2 
REALM .ViANXT(4,l),MANHlXrr,MANSPDT 
INTEGER TIME,DMEM1,MANTIME 


C DEHNE THE INPUT AND OUTPUT HLES 

OPEN(UNIT=3,HLE='NOISEnL'^ATUS='OLI>') 

OPEN(UNrr=4,FILE='TOKDATA'^ATUS='NEW') 

WRITE(*/)'ENTER A NEGATIVE NUMBER FOR NOISELESS CASE/ 
WRITE(*,*)TOSITIVE FOR NOISY CASE' 

read(*/x:ase 

TIMEM1=0 

RTOD=57.29577951 

DTOR=0.017453293 

C INPUT THE TARGET TRACK PARAMETERS 

WRrrE(*,»)'INPUT DESIRED INITIAL X POSITION OF TARGET' 
READf*,*) XT(1,1) 

WRITE(*,*)'INPLrr DESIRED INITIAL Y POSITION OF TARGET' 
READ(*,*) XT(3,1) 

WRITE{*,*)'LNPLU’ DESIRED TARGET COURSE IN DEGREES' 
READ{*,*)HDGT 

WRITE(*,*)'INPUT DESIRED TARGET SPEED IN KNOTS' 
READ(*,*)SPDT 







XT{2,1 )=(SPDT/60)‘SIN(HDGT*DTOR) 
XT(4,1 )=(SPDT/60)*COS(HDGT*DTOR) 


C INPUT THE SENSOR TRACK PARAMETERS 

\VRITE(*,*)'FOR SENSOR 1;' 

WRITEU,*)TNPUT DESIRED INITIAL X POSITION' 

READ(V)XS1{1,1) 

WRITE(*,*)'INPUT DESIRED INITIAL Y POSITION' 

READ(V)XS1(3,1) 

WTUTE(*,’)'INPUT DESIRED COURSE IN DEGREES' 

READU,*)HDGS1 

WRITE(*,*)'INPUT DESIRED SPEED IN KNOTS' 

READ(*,*)SPDS1 

XSl (2,1 )={SPDS1 /60)-^IN(HDGS1*DTOR) 

XSl (4,1 )={SPDS1 /60)*COS(HIX;S1 *DTOR) 

vv'RITE(*,*)'FOR SENSOR 2;' 

\VRITE{»,*)'INPUT DESIRED INITIAL X POSITION' 

READ(*,*)XS2(L1) 

WRITE(*,*)'INPUT DESIRED INITIAL Y POSITION' 

READ(*,*)XS2(3,1) 

WRrrE(*,*)'INPUT DESIRED COURSE IN DEGREES' 

READ(*,*)HDGS2 

WRITE(*,‘)'INPUT DESIRED SPEED IN KNOTS' 

READ(*,*)SPDS2 

XS2(2,I )={SPDS2/60)'^IN(HDGS2*DTOR) 

XS2(4,1 )={SPDS2/60)*COS(HDGS2*DTOR) 

C INPUT THE TRUE TRACK UPDATES 

300 WRITEC *)'INPUT TIME INTERVAL OF CALCULATIONS' 
VVRrTE{»,*)'(NEG.FOR END OF PROBLEM)' 

READ{*,*)TIME 

WRITE( V)'ENTER TIME FOR TARGET MANEUVER IF SO 
\VRITE{*,*)'IF NOT MANEUVER ENTER O' 

READ{V)MANTIME 

IF (MANTIME.EQ.0) <30T0 400 

IF (MANTIME-NE-O) THEN 

WRITEC,*)'INPLrr MANEUVERING TARGET COURSE LN DEGREES' 
READ{*,*)MANHDGT 

VVRrrE(*,*)'INPUT MANELiVERING TARGET SPEED IN KNOTS' 
READ(V)MANSPDT 
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MANXT(2,1)=(MANSPDT/60)»SIN(MANHDGT*DTOR) 

MANXT(4,l)=(MANSPDT/60)»COS(MANHDG'rDTOR) 

ENDIF 


C UPDATE TARGET AND SENSOR STATES TO MEASUREMENT TIME 

400 DT=TIME-TIMEM1 

C COMPUTE PHI MATRIX 
PHI(l,l)=l-0 
PHI(1,2)=DT 
PHI(13)=0.0 
PHI(1,4)=0.0 
PHI(2,1)=0.0 
PHI{2,2)=1.0 
PHI(23)=0.0 
PHI(2,4)=0.0 
PHI(3,1)=0.0 
PHI(3,2)=0.0 
PHI(3,3)=1.0 
PHI{3,4)=DT 
PHI{4,1)=0.0 
PHI(4,2)=0.0 
PHI(4,3)=0.0 
PHI(4,4)=1.0 

C INmATE THE PROCESS OF TRACK DATA GENERATION 
DO 310 1=1,30 
TIME = PDT-1 

IF (MANTIME-EQ.O) GOTO 600 
IF {TIME.EQ.MANTIME) THEN 
XT(2,1)=MANXT(2,1) 

XT(4.1)=MANXT(4,1) 

ENDIF 

C UPDATE TARGET STATES 

600 CALL MATMUL(PHLXT,4,4,1,TEMP1) 

DO 700 K=l,4 

XT(K,1)=TEMP1{K,1) 

700 CONTINUE 
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C UPDATE SENSOR STATES 


CALL MATMUL(PHI,XST4,4.1,TE.MP1) 
DO 710 L=1,4 

XS1(L,1)=TEMP1(L,1) 

710 CONTINUE 


CALL MATMUUPHLXS2,4ATTEMP1) 
DO 720 M=l,4 

XS2(M,1 )=TEMP1{M,1 ) 

720 CONTLNUE 

XDIFFl =XTCl,l )-XSl( 1,1) 

YDIFF1 =XT{3,1 )-XSl (3,1) 


XDIFF2=XT(1,1>-XS2(1,1) 
\'DIFF2=XT(3,1 )-XS2<3,l) 

READ(3.*) NOISE 


IF (CASE.GE.0.0) GOTO 450 
NOISE=0.0 


450 BRGl =RTOD*ATAN2{XDIFFl,YDIFn)+NOISE 
IF (BRGI.LT.0.0) BRGl =BRG1+360 
BRG2=RTOE)*ATAN2(XDIFF2,YDIFF2)+NOISE 
IF (BRG2.LT.0.0) BRG2=BR(S2+360 


WRrrE(4,500)TIME,XT(l ,1),XT(3,1 ),XS1(1,1),XS1(3,1), 
• BRG1,XS2{1,1),XS2(3,1),BRG2 

500 FORMAT(I4,8F9.4) 

310 CONTINUE 

900 STOP 


END 
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n n n o n 


SUBROUTINE MATMUL(A,B,L,M,N,C) 

THIS ROUTINE MULTIPLIES TWO MATRICES TOGETHER 
C(L,N) = A(L,M) » B(M,N) 

DIMEIn'SIONS AND DECLARATIONS 

REALM A(L,M),B(M,N),C(L,N) 

DO 10 I=1,L 
.O10J=l,N 
C(I,J)=0.0 
10 CONTINUE 


DO 100 1= 1,L 
DO 100 J= UN 
DO 100 K= 1,M 
C(I,J) = C(IJ) + AaK)’^B(KJ) 
100 CONTINUE 

RETURN 

END 
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APPENDIX B. PROGRAM SHIPTRACK 


C 


»**SHIPTRACK»** 




THIS PR(X;RAM employs an ADAimVE EXTENDED KALMAN HLTER TO 
TRACK A MANEUVERING BOAT TARGET USING BEARINGS-ONLY RADIO 
DIRECTION-FINDING MEASUREMENTS FROM TWO INDEPENDENT SENSORS 


^»*»*if»»*»j(*ifif»»*#»****»»*»»»******»*»>Ht#»*iHHt»iH*.jf*Jt**»»»iHt****»»iHf*»»»JHHHt*»****»**»K.*****lfif»*»*»»*lf*lf 


C VARIABLE DEFINITIONS 


C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


BRG = MEASURED TARGET BEARING IN RADIANS 
BRKKMl PREDICTED TARGET BEARING MEASUREMENT IN RADIANS 
BRG(k/k-l) 

DBRG = MEASURED TARGET BEARING IN DEGREES 
DEL = STATE NOISE COEFFICIEInJT MATRIX 
DFACC = DF ACCURACY OF ESM SYSTEM 
DT = TIME DELAY BETWEEN OBSERVATIONS, T(k)-T(kl) 

DTOR = DEGREE TO RADIAN CONVERSION FACTOR 
E = OBSERVATION RESIDUAL, BRG-ATAN(XDIFF/YDIFF) 

E1,E2 = IvIE/.SUREMENT RESIDUAL, Z(k)-H-^X(k/k-l) 

E]M1,E2M1 = MEASUREMENT RESIDUAL AT PREVIOUS OBSERVATION 
E1M2,E2M2 = MEASUREMENT RESIDUAL TWO PREVIOUS OBSERVATIONS 
FACl = RECIPROCAL OF VARIANCE OF RESIDUALS (VARE) 

G KALMAN GAIN VECTOR 

GATE! = 1.5-^TANDARD DEVIATION OF RESIDUAL PROCESS, 

USED \S A GATE IN MANEUVER DETECTION 
H = MEASUREMENT MATRIX 

HDG = ESTIMATED TARGET HEADING IN DEGREES 

HT = TRANSPOSE OF H 

I = COUNTER 

IMAT =4X4 IDENTITY MATRIX 
] = COUNTER 

K = ITEPJ^TION lOTERVAL 

L = SENSORS, L = 1 and 2 

LPKK = STATE COVARIANCE MATRIX AFTER PREVIOUS 
OBSERVATIONS 

LPKKMl = A PRIORI STATE COVARIANCE ESTIIvlATE 
LXKK = STATE ESTIMATE AFTER PREVIOUS OBSERVATIONS 
LXKKMl = A PRIORI STAl’E ESTIMATE 
Ml .M2 = AVERAGE OF RESIDUALS OVER LAST THREE 

OBSERVATIONS 

OBSERR = OBSERVATION ERROR 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


PHI = DISCRETE-TIME STATE TRANSITION MATRIX 

PHIT = TRANSPOSE OF PHI 

PI = 3.141592654 

PKK = ESTIMATION ERROR COVARIANCE MATRIX, P(k/k) 

PKKMl = PREDICTED ESTIMATION ERROR COVARIANCE MATRIX, 
P(k/k-l) 

Q = STATE EXCITATION COVARIANCE MATRIX 

R = MEASUREMENT NOISE COVARIANCE 

RANGE = DISTANCE FROM SENSOR TO A PRIORI TARGET 
POSITION 

RTOD = RADIAN TO DEGREE CONVERSION FACTOR 
SPD = ESTIMATED TARGET SPEED IN KNOTS 
TEMP = TEMPORARY STORAGE MATRICES USED IN MATRIX 
OPERATIONS 

TIME = ACTUAL OBSERVATION TIME 
TIMEMl = TIME COUNTER 
TRKERR = TRUE TRACKING ERROR 
VARE = VARIANCE OF RESIDUALS PROCESS 
XDIFF = DISTANCE IN X DIRECTION FROM SENSOR TO A PRIORI 
TARGET POSITION 

XKK = ESTIMATED TARGET STATE VECTOR, X(k/k) 

XKKMl = PREDICTED TARGET STATE VECTOR, X(k/k-l) 

XPOS = ESTIMATED TARGET POSITION IN X DIRECTION 
XS = POSITION OF SENSOR IN X DIRECTION 
XT = TRUE TARGET POSITION IN X DIRECTION 
YDIFF = ’^’‘'TANCE IN Y DIRECTION FROM SENSOR TO A PRIORI 
TARGET POSITION 

YPOS = ESriMAT-ED TARGET POSITION IN Y DIRECTION 
YS = POSITION OF SENSOR IN Y DIRECTION 
YT = TRUE TARGET POSITION IN Y DIRECTION 
ZX = OBSERVED POSITION IN X DIRECTION 
ZY = OBSERVED POSITION IN Y DIRECTION 


C VARIABLE DECLARATIONS 

REAL XKK(4,l),XKKMl(4,l),PHI(4,4),DTOR 

REAL H(1,4),G(4,1),TEMP1(1,4),TEMP2(1,1),TEMP3(4,1) 

REALTEMP4(4,4),TEMP5(4,4),PKK(4,4),PKKM1(4,4),HT(4,1) 

REAL LXKK(4,1),LPKK(4,4),XS(2),YS{2),DBRG(2),BRG(2) 

REAL TEMP6(4,4),TEMP7(4,4),PHIT(4,4),IMAT(4,4) 

REAL XT,YT,GATE1 (2),DT,XDIFF,YDIFF,RANGE,Q(4,4) 

REAL XS1,YS1,BRG1,BRKKM1,E(2),VARE{2),FAC1 
REALR,DFACC,RTOD,YS2,BRG2,ZX,ZY,Ml,El,ElMl,ElM2 
REALLPKKM1(4,4),TRKERR,M2,E2,E2M1,E2M2,G11,G13,G21,G23 
REALLXKKMl(4,l),ZXMl,ZYMl,OBSERR,DEL(4,2) 

INTEGER TIME,TIMEM1 
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C OPEN OUTPUT DATA HLES 


OPEN(UNIT=2,FILE='OUTDATA',STATUS='NEW') 

OPEN(U>;iT=3,nLE='TRKDATA',STATUS='OLD') 

OPEN(UNIT=4,FILE='ERRDATA',STATUS='NEW') 

OPEN(UNIT=5,nLE='TRKINFO',STATUS='NEW') 

C RADIAN/DEGREE CONVERSION FACTORS 

RTOD=57.29577951 

DTOR=0.01745293 

C COMPUTE 4X4 IDENTITY MATRIX 

DO 10 1=1,4 
DO 10 J=l,4 
IF (I.EQJ) THEN 
IMAT(I,J)=1.0 

ELSE 

IMAT(I,J)=0.0 

ENDIF 

10 CONTINUE 

C INITIALIZE TIME COUNTER 
TIMEM1=0 

C INITIALIZE COUNTER FOR MANEUVER GATE 
E1M1=0.0 
E1M2=0.0 
E2M1=0.0 
E2M2=0.0 

C COMPUTE BEARING MEASUREMENT COVARIANCE 
C BEARING ERROR STANDARD DEVIATION = DF ACCURACY OF SYSTEM 
WRITE(*,*)'ENTER DF ACCURACY OF ESM SYSTEM' 

READ(*,*)DFACC 

R=(DFACC*DTOR)**2 


C READ IN OBSERVATION PACKET (TIME, # OF SENSORS) 
C DT=TIME(k)-TIME(k-l) 

20 KF ' D' ',30,END=240)TIME,X'r,YT,XS(l),YS(l),DBRG(l), 

» vs(2),YS(2),DBRG(2) 

30 FOi<isiAT(I4,8F9.4) 
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DO 40 L=l,2 

IF (DBRG(L).GT.180.0) DBRG(L)=DBRG(L)-360 
BRG(L)=DBRG(L)*DTOR 

40 CONTINUE 

IF (TIME.LT.O) GOTO 240 

DT=TIME-T1MEM1 

C CALCULATE THE PHI MATRIX 

CALL HNDPHKPHLDT) 

XS1=XS(1) 

YS1=YS(1) 

XS2=XS(2) 

YS2=YS(2) 

BRG1=BRG(1) 

BRG2=BRG(2) 

C COMPUTE THE TARGET POSITION FROM BEARING MEASUREMENTS 
CALL MP(XS1,YSLXS2,YS2,BRG1,BRG2,ZX,ZY) 

IF(TIME.EQ.O) THEN 

CALL INmXSl,YSl,XS2,YS2,BRGl,BRG2,XKK,PKK) 

C WRITEC,*)'X(0/0,0):' 

DO 50 1=1,4 

LXKK(I,1)=XKK(I,1) 

C WRITE(V)XKK(I,1) 

50 CONTINUE 

C WRITE(*,»)'P(0/0.0):' 

DO 70 1=1,4 
DO 70 J=l,4 
LPKK(I,J)=PKK(I,J) 

C WRITE(*,60)PKK(I,J) 

60 FORMAT{4n4.4) 

70 CONTINUE 

ENDIF 

C PROJECT AHEAD THE STATE ESTIMATE 
C X(k+l/k) = PHI*X(k/k) 

CALL MATMUL(PHI,XKK,4,4,1,XKKM1) 
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C WRITEe,»)'X(',TIME//',TIMEMl/,0);' 

DO 80 I==l,4 

C WRITE(^*)'XKKM1(I,1) 

LXKKM1(I,1)=XKKM1(U) 

80 CONTINUE 

C PROJECT AHEAD THE ERROR COVARIANCE ESTIMATE 
C P{k+l/k) = (PHI*P(k/k)*PHIT) + Q 

CALL FINDDEL(DEL,DT) 

CALL MATRAN(PHI,PHIT,4,4) 

CALL MATMUL(PHI,PKK,4,4,4,TEMP6) 

CALL MATMUL(TEMP6,PHIT,4,4ATEMP7) 

CALL GETQ(XKKM1,DEL,Q) 

CALL MATADD(TEMP7,Q,4,4,1,PKKM1) 

DO 90 1=1,4 
DO 90 J=l,4 

LPKKM1(I,J)=PKKM1{I,J) 

90 CONTINUE 

C WRITE(*,*)T(',TIME,7',TIMEM1,',0):' 

DO 110 1=1,4 

C WRITE(M00)(PKKM1(I,J)J=1,4) 

100 FORMAT(4F14.4) 

110 CONTINUE 
120 CONTINUE 

DO 230 L=l,2 

c WRITE!*, ♦)'»»**prck:essing measurement # ',L/ ****' 

XDIFF=XKKM1 (1,1 )-XS(L) 

YDIFF=XKKM1(3,1)-YS(L) 

RANGE=SQRT(XDIFP*2+YDIFP*2) 

C UPDATE H MATRIX WITH LATEST STATE ESTIMATES 
H(l,l )=YDIFF/RANGE**2 
H(l,2)=0.0 

H(1,3)=-XDIFF/RANGE**2 

H(l,4)=0.0 


C COMPUTE OBSERVATION RESIDUAL 
BRKKMl =ATAN2(XDIFF,YDIFF) 
E(L)=BRG(L)-BRKKM1 
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C COMPUTE VARIAinCE OF RESIDUALS SEQUENCE 
C AND ADAPTIVE GATE VALUE 
C VAR(E)=H*PKKMrHT+R 

CALL MATRAN(H,HT,1,4) 

CALL MATMUL(H,PKKM1,1,4ATEMP1) 
CALL MATMUL(TEMP1,HT,1,4,1,TEMP2) 
VARE(L)=TEMP2(1,1)+R 

GATE1(L)=1.5»SQRT(VARE(L)) 

C COMPUTE KALMAN GAIN MATRIX 
C G=PKKM1*HT*(H*PKKMPHT+R)**(-1) 

CALL MATRAN(H,HT,1,4) 

CALL MATMUL(PKKMLHT,4A1,TEMP3) 

C WRITE(*,*)TKKM1*HT =' 

DO 130 1=1,4 

C WRITE(*,*)TEMP3(I,1) 

130 CONTINUE 

FAC1=1/VARE(L) 

CALL MATSCL(FAC1,TEMP3,4,1,G) 

C WRITE(*,‘^)'G(k) =' 

DO 140 1=1,4 

C WRITE(*,*)G(I,1) 

140 CONTINUE 

IF (L.EQ.l) THEN 
G11=G(1,1) 

G13=G(3,1) 

ELSE 

G21=G(1,1) 

G23=G(3,1) 

ENDIF 


C COMPUTE UPDATED ESTIMATE 

C X(k/k) = X(k/k-l) + G * E, WHERE E=Z(k)-H(k)*X(k/k-l) 

XKK(1,1)=XKKM1(1,1)+(G(1,1)*E(L)) 

XKK(2,1 )=XKKM1 (2,1 )+(G(2,l )*E(L)) 

XKK(3,1 )=XKKM1 (3,1 )+(G(3,l )*E(L)) 
XKK(4,1)=XKKM1(4,1)+(G(4,1)*E(L)) 
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C WRITE(*,*)'X(',TIME/ /',TIME//,L/);' 

DO 150 1=1,4 

C WRITE(*,»)XKK{I,1) 

150 CONTINUE 

C COMPUTE UPDATED ERROR COVARIANCE MATRIX 
C P(k/k) = (I - G»H) * P(k/k-l) 

CALL MATMUL(G,H,4,1,4,TEMP4) 

C WRITE(»,*)'G*H =' 

DO 170 1=1,4 

C WRITE(M60)(TEMP4{I,J),J=1,4) 

160 FORMAT(4F14.4) 

170 CONTINUE 

CALL MATSUB(IMAT,TEMP4,4,4,TEMP5) 

C WRITEC,*)'I-G»H =' 

DO 190 1=1,4 

C WRJTE(M80)(TEMP5(I,J),J=1,4) 

180 FORMAT(4n4.4) 

190 CONTINUE 

CALL MATMUL(TEMP5,PKKM1,4,4,4,PKK) 

C WRITE(»,*)T(',TIME,7','nME,'/,L,'):' 

DO 210 1=1,4 

C WRITE(^200)(PKK(I,J),J=1,4) 

200 FORMAT(4F14.4) 

210 CONTINUE 

C IF THERE ARE MORE MEASUREMENTS 
IF (L.LT.2) THEN 

C USE UPDATED STATE AND ERROR COVARIANCE 
C ESTIMATES FOR NEXT MEASUREMENT 
DO 220 1=1,4 
DO 220 J=l,4 

PKKM1(I,J)=PKK(I,J) 

XKKM1(I,1)=XKK(I,1) 

220 CONTINUE 
ENDIF 

230 CONTINUE 
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C COMPUTE TRUE TRACKING ERROR 

TRKERR=SQRT((XT-XKK(1,1))**2+(YT-XKK(3.1))*’^) 


C COMPUTE OBSERVATION ERROR 

OBSERR=SQRT<(XT-ZX)**2+(YT-ZY)*-^) 


C SAVE LATEST RESIDUALS FOR AVERAGING 
E1=E(1) 

E2=E(2) 


C COMPLT’E THE A\^RAGE RESIDUAL OVER THE PAST THREE OBSERVATIONS 
Ml=(E1 +E1 Ml +E1 M2)/3 
M2=(E2+E2Ml+E2M2)/3 


C PAST THREE RESIDUALS FOR SENSOR 1 ARE ; E1,E1M1,E1M2 
C BEARING AVERAGE OF SENSOR 1 = Ml 
C MANEU\^R GATE FOR SENSOR 1 = GATEl(l) 

C PAST THREE RESIDUALS K)R SENSOR 2 ARE : E2,E2M1,E2M2 
C BEARING AVERAGE OF SENSOR 2 = M2 
C MANEUVER GATE FOR SENSOR 2 = GATE1(2) 

E1M2=E1M1 

E2M2=E2M1 

E1M1=E1 

E2M1=E2 


C COMPLTTE ESTIMATED X-Y POSITION, COURSE, AND SPEED 
XPOS=XKK(l,l) 

^TOS=XKK(3,l) 

IF (XKK(2,l).EQ.O .AND. XKK(4,l).EQ.O) THEN 
HDG = 0.0 
ELSE 

HDG=RTOD*ATAN2(XKK(2,l ),XKK(4,1)) 

ENDIF 

IF (HDG.LT.0.0) HDG=HDG+360 
SPD=60*SQRT(XKK(2,1 >**2+XKK(4,l)**2) 
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C WRITE DATA IN OUTPUT RLES 


WRITE(2,*)TIME,XPOS,YPOS,ZX,ZY 

WRITE(4,*)TIME,TRKERR,OBSERR 

WRITE(5,*)TIME,XPOS,YPOS,HDG,SPD 


C COMPARE BEARING ERRORS TO MANEUVTER DETECTION GATES 

IF ((ABS(Ml).GT.(GATEl(l))).OR. 

* {ABS(M2).GT.(GATE1(2)))) THEN 

WRITE{*,*)'***MANEUVER DETECTION***' 

CALL REINIT(DT,ZX^,ZXM1,ZYM1,LPKKM1,XKKM1,PKKM1) 
E1M1=0.0 
E1M2=0.0 
E2M1=0.0 
E2M2=0.0 
GOTO 120 
ENDIF 

TIMEM1=TIME 

ZXM1=ZX 

ZYM1=Z^' 

GOTO 20 

240 CLOSE(LJNIT=2) 

CLOSE(UNrr=3) 

CLOSE(UNIT=4) 

CLOSE(UNIT=5) 

STOP 

END 
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C SUBROlTiiNES 


SUBROUTINE FlNDPHl(PHI.DT) 

C COMPUTES THE VALUES OF THE PHI MATRIX 
REAL PHI(4,4),DT 


DO 500 1=1,4 
DO 500 J=l,4 
DO 500 K=l,2 
PHI{!,J)=0.0 
500 CONTINUE 

C COMPUTE PHI MATRIX 
DO 510 1=1,4 
PHI{I,I)=1.0 
510 CONTINUE 
PHI(1,2)=DT 
PHI(3,4)=DT 

RETURN 

END 


SUBROUTINE GErQ(XKKMl,DEL,Q) 


C SUBROinWE TO GET Q MATRIX 


REAL XKKM1(4,1),Q<4,4) 

REAL QPR{2,2),DEU4,2),DELT(2,4) 
REAL VARV,VARTH,VT 


C INTEGER FLAG 


IF {(XiaCMl(2,l).EQ.O).OR.(XKKMl(4,l).EQ.O)) THEN 
DO 520 1=1,4 
DO 520 J=l,4 
Q<I,J)=0.0 
GOTO 530 
520 CONTINUE 
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ENDIF 


C CALCULATE O' MATRIX 

C LINEAR ACCELERATION = 0.0005 nin/(hr)'"2 
VARY =0.001 

C ANGULAR ACCELERATION = 0.001 rad/(hr)^2 
VARTH = 0.001 

\T=SQRT(XKKM1 (2,1 )”2+XKKMl{4,l)**2) 
QPR{l,l)=(((XKKMl(2,T)/VD**2rVARV)+({XKKMl(4,l)**2)*VARTH) 
QPR(2,2)=«(XKKMl(4,l)/\T)*'^)*VARV)+((XKKMl(2,l)**2)*VARrH) 
QPR(1,2)=((XKKM1(2,1))*{XKKM1(4,1))/(\T^*2))*VARV 
* -(XKKMl(2,l))*(XKiail(4,l))*VARTH 
QPR(2,1)=QPR(1,2) 

C Q=DEL(K)*Q'(K)*DELT(K) 

CALL MATRAN(DEL,DELT,4,2) 

CALL MATMUL(DEL,QPR4,2,2,TEMP10) 

CALL MATMUL(TEMP10,DELT,4,2,4,Q) 

C CALL MATSCL(;0.01,a4,4,Q) 

530 RETURN 

END 


SUBROLiTINE FINDDEL(DEL,DT) 


C COMPUTE THE VALUES OF THE DEL MATRIX 

f^***»»**»*****»9**»»9*»******»»»f*******»*»9**»*»^ 


REAL DEL(4,2),DT 


DEL{l,l)=DT**2.,/2. 

DEL(1,2)=0 

DEL(2,1)=DT 

DEL(2,2)=0 

DEL(3,1)=0 

DEU;3,2)=I7P*2./2. 

DEL(4,1)=0 

DEL(4,2)=DT 


RETURN 

END 
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SUBROUTINE INIT(XS1,YS1,XS2,YS2,BRG1,BRG2,XKK,PKK) 




C THIS ROUTINE INITIALIZE THE STATE AND ERROR 
C COVARIANCE ESTIMATES 




REAL XKK(4,1), PKK{4,4) 

REAL XS1,YS1,XS2,YS2,BRG1,BRG2 
REAL NUMER,DENOM 


C INITIAL STATE ESTIMATE 


NUMER=(-YS2*TAN(BRG2))+(YS1TAN(BRG1))+XS2-XS1 

DENOM=TAN{BRGl)-TAN{BRG2) 

XKK(3,l)=NUMER/DENOM 

XKK(2,1)=0.0 

XKK(1,1)=(XKK(3,1)-YS1)»TAN(BRG1)+XS1 

XKK(4,1)=0.0 


C LNITIAL ERROR COVARIANCE ESTIMATE 

PKK(L1)=10000 

PKK(l,2)=G.O 

PKK(L3)=0.0 

PKK(1,4)=0.0 

PKK(2,1)=0.0 

PKK(2,2)=0.2500 

PKK(2,3)=0.0 

PKK(2,4)=0.0 

PKK(3,1)=0.0 

PKK(3,2)=0.0 

PICK(3,3)=10000 

PKK(3,4)=0.0 

PKK(4,1)=0.0 

PKK(4,2)=0.0 

PKK(4,3)=0.0 

PKK(4,4)=0.2500 

RETURN 

END 
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SUBROUTINE REINIT(DT,ZX,ZY,ZXM1,ZYM1,LPKKM1,XKKM1,PKKM1) 

C THIS ROUTINE RE-INITIALIZES THE STATE AND ERROR 
C COVARIANCE ESTIMATE 

REAL DT,XKKM1(4,1),PKKM1(4,4) 

REAL ZX,ZY^Ml,ZYMl,LPKKMi(4,4) 

XDIFF=ZX-ZXM1 

YDIFF=Z\'-rk'Ml 

XKKM1(1,1)=2:X 

XKKM1(2,1)=XDIFF/DT 

XKKM1(3,1)=Z^' 

XKKMl(4,l)='i'DIFF/DT 

C VVRITE(3,*)'REINmALIZED STATES ARE :' 

DO 540 1=1,4 

C VVRITE(3,*)XKKMl(I,i:) 

540 CONTINUE 

PKKMl (1,1 )=225*LPICKM1 (1,1) 

PKKM1<1,2)=0.0 

PKKM1(1,3)=2.25*LPKKM1(U) 

PKICM1(1,4)=0.0 
PKKM1(Z1)=0.0 
PKKM1{Z2)=0.1111 
PKKMl {Z3)=0.0 
PKKMl (2,4)=0.0 
PKKM1(3,1)=2.25*LPKKM1<3,1) 

PKKMl (3,2)=0.0 

PKKMl (3,3)=225*LPKKM1 (3,3) 

PKKMl (3,4)=0.0 
PKKMl (4,1 )=0.0 
PKKMl (4,2)=0.0 
PKKM1(43)=0.0 
PKKMl (4,4)^.1111 

RETURN 

END 
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SUBROUTINE MP(XS1,YS1,XS2,YS2,BRG1,BRG2,ZX,7Y) 


C THIS ROUTINE COMPUTES THE ESTIMATED 

c x,Y posrnoN obtained from measurements 




REAL ZX,ZY 

REAL XSl,YSi,XS2,YS2,ERGl,BRG2 
REAl NUMER,DEN0M 


C INITIAL STATE ESTIMATE 


NUMER=(-YS2*TAN(BRG2))+(YSlTAtvI(BRGl))+XS2-XSl 

DEN0M=TAN(BRG1)-TAN(BRG2) 

ZY=NUMER/DENOM 
ZX=(ZY-YS1 )*TAN(BRG1 )+XS1 

RETURiN 

END 


SUBROUTINE MATMULIA^B.UM.rC^C) 


C THIS ROUTINE MULTIPLIES TWO MATRICES TOGETHER 
C C(L,N) = A(L,M) ♦ B(M,N) 




C DIMENSIONS AND DECLARATIONS 
REAL A(L,M),B(M,NI,C(L,N) 


DO 570 1=1,L 
DO 570 J=1,N 
C(IJ)=0.0 
570 CONTINUE 


DO 580 1=1 ,L 
DO 580 J=1,N 
DO 580 K=1,M 
C(I,J) = C(U) + A(I,K)*B(K,j; 
580 CONTINUE 


RETURN 

END 
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SUBROUTINE MATRAN(A,B,N,M) 




C THIS ROUTINE TRANSPOSES A MATRIX 
C B(M,N) = A'(N,M) 




C DIMENSIONS AND DECLARATIONS 
REAL A(N,M), B(M,N) 

DO 590 1=1 ,N 
DO 590 J=1,M 
B0,I) = A(IJ) 

590 CONTINUE 
RETURN 
END 


SUBROUTINE MATSCL(Q,A,N,M,C) 

C THIS ROUTINE MULTIPLIES A MATRIX WITH A SCALAR 
C C(N,M) = Q » A(N,M) 

^»»**iHt**>f*»***»***»******iHf»****»*iHf»»*»iHt*if*»»»**»if»*»***T>f*** 

C DIMENSIONS AND DECLARATIONS 
REAL A(N,M), C(N,M) 

DO 600 1=1,N 
DO 600 J=1,M 
C(I,J) = Q*A(I,J) 

600 CONTINUE 
RETURN 
END 


SUBROu llNb MaTADD(A,B,N,M,L,C) 




C THIS ROUTINE ADDS TWO MATRICES 
C { C(N,M) = A(N,M) + B(N,M) I 




C DIMENSIONS AND DECLARATIONS 
REAL*4 A(N,M),B(N,M),C(N,M,L) 

DO 610 I = 1,N 
DO 610 J = 1,M 
C(I,J,L)=A(IJ)+B(I,J' 

610 CONTINUE 
PRTURN 
END 
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SUBROUTINE MATSUB(A,B,N,M,C) 


C THIS ROUTINE SUBTRACTS TWO MATRICES 
C C(N,M) = A(N,M) - B(N,M) 




C DIMENSIONS AND DECLARATIONS 
REAL A(N,M),B(N,M),C(N,M) 


DO 620 I=1,N 
DO 620 J=1,M 
C(I,J)=A(IJ)-B(IJ) 
620 CONTINUE 


RETURN 


END 
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APPENDIX C. FUNCTION PLOT 


% Function PLOT 
% 

% This function will plot the results obtained from the program 
% SHIPTRACK.FOR which compute the estimation process based on the 
% Extended Kalman Filter equations 

% 

load TRKDATA.DAT 
load ERRDATA.DAT 
load OUTDATA.DAT 

% 

% Plotting the Tracking and Observed errors 
% 

%axis([0 max(ERRDATA(:,l)) 0 max(ERRDATA(:,2))+0.1]) 
plot(ERRDATA(:,l),ERRDATA(:,2), 

ERRDATA(:,1),ERRDATA(;,3)/-/) 
xlabeU'TlME (minutes)') 
ylabeU'ERROR (nautical miles)') 

title('(TRACKING ERROR (-)),(OBSERVED ERROR (-.)) vs t/) 

grid 

pause 

meta RESULTS 
% 

% Plotting the tracking process 
% 

axisdO 25 0 25]); 

plot(TRKDATA(:,2),TRKDATA(:,3)/-',... 

TRKDATA(:,4),TRKDATA(;,5)/+g',... 

TRKDATA(:,7),TRKDATA(:,8)/+r',... 
OUTDATA(:,4),OUTDATA(:,5)/+b',..:. 
OUTDATA(:,2),OUTDATA(:,3)/ow') 
gtexK'Patrol Boat T),pause 
gtexK'Patrol Boat 2'),pause 
gtexK'TRUE TRAJECT. ; -') 
gtext('OBS.POSmONS : +') 
gtext('FST. POSITIONS : o ') 
xlabeK'X coordinate (Nautical Miles)') 
ylabeK'Y coordinate (Nautical Miles)') 

Htle('TRACKlNG PR(X:ESS') 

grid 

pause 

meta RESULTS 
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% 

% Plotting the scenario 
axis([0 25 0 25]); 

plot(TRKDATA(:,2),TRKDATA(:,3)/o',. 
TRKDATA(:,4),TRKDATA(:,5)/+g',... 
TRKDATA(:,7),TRKDATA(;,8)/+r') 
xlabeK'X coordinate (Nautical Miles)') 
ylabel('Y coordinate (Nautical Miles)') 
title('SCENAR10 Nr. _') 
gtext('Patrol Boat l'),pause 
gtext('Patrol Boat 2'),pause 
gtext('Start'),pause 
gtext('End'),pause 
grid 
pause 

meta RESULTS 
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35660 Para de Minas - MG 
Brasil 

18. LTC Carlos Augusto Teixeira 1 

Rua Farani 60 - Ap 904 

Rip de Janeiro - RJ 
Brasil 

19. LCDR Sudjiwo 1 

Kompleks Tru-Al No. 17B 

Radio Dalam 
Kebayoran Baru 
Jakarta-Selatan 12140 
Indonesia 

20. CDR Uwe Becker I 

c/o Federal Republic of Germany 

Military Representative, Si - pers.o.V.i.A. 

4000 Brandywine St., N.VV. 

Washington, DC. 20016-1887 


21. CDR Chen-Kuo Yu 
4F-2,282, Timg-Men Rd. 
Ku-Shan, Kaohsiimg 
Taiwan 80401 
Republic of China 
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